#ifndef __SV2_ARUCO_DET__
#define __SV2_ARUCO_DET__

#include "sms_core.h"
#include <opencv2/opencv.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/tracking.hpp>
#include <string>
#include <chrono>
#include <mutex>
#include <queue>
#include <condition_variable>


/*
依赖安装
sudo apt install libopencv-dev

*/


namespace sv2 {


class AutoFocusDetCppNode : public sms::BaseNode
{
public:
  AutoFocusDetCppNode(
    std::string job_name,
    std::string full_det_name,
    std::string roi_det_name,
    std::string param_file,
    std::string ip="127.0.0.1",
    int port=9094
  ) : sms::BaseNode("AutoFocusDetCppNode", job_name, param_file, ip, port), 
    _full_res_sub("/" + full_det_name + "/detector/image_results", "sensor_msgs::CompressedImage", std::bind(&AutoFocusDetCppNode::full_res_callback, this, std::placeholders::_1)),
    _roi_res_sub("/" + roi_det_name + "/detector/image_results", "sensor_msgs::CompressedImage", std::bind(&AutoFocusDetCppNode::roi_res_callback, this, std::placeholders::_1)),
    _full_det_ctl_pub("/" + full_det_name + "/detector/control", "std_msgs::Null"),
    _roi_det_ctl_pub("/" + roi_det_name + "/detector/control", "std_msgs::Null"),
    _res_pub("/" + job_name + "/autofocus/results", "spirecv_msgs::2DTargets"),
    _res_img_pub("/" + job_name + "/autofocus/image_results", "sensor_msgs::CompressedImage")
  {
    // 读取节点参数
    this->_lock_thres = this->get_param("lock_thres", 5);
    this->_unlock_thres = this->get_param("unlock_thres", 5);
    this->_lock_scale_init = this->get_param("lock_scale_init", 12.0);
    this->_lock_scale = this->get_param("lock_scale", 8.0);
    this->_keep_unlocked = this->get_param("keep_unlocked", false);
    this->_use_square_region = this->get_param("use_square_region", false);
    nlohmann::json categories_filter = this->get_param("categories_filter", {});

    logger.info("lock_thres: " + std::to_string(this->_lock_thres));
    logger.info("unlock_thres: " + std::to_string(this->_unlock_thres));
    logger.info("lock_scale_init: " + std::to_string(this->_lock_scale_init));
    logger.info("lock_scale: " + std::to_string(this->_lock_scale));

    logger.info("keep_unlocked: " + std::to_string(this->_keep_unlocked));
    logger.info("use_square_region: " + std::to_string(this->_use_square_region));
    logger.info("categories_filter: " + categories_filter.dump());

    // 将读取到的参数转换到成员变量：_categories_filter
    for (const auto& item : categories_filter) { this->_categories_filter.push_back(item); }

  }
  ~AutoFocusDetCppNode()
  {
  }
  void run();

  void full_res_callback(nlohmann::json msg)
  {
    // 放入到阻塞队列中
    {
      std::unique_lock<std::mutex> lock(this->_full_queue_mtx);
      this->_full_queue.push(msg);
    }
    // 通知主线程取数据
    this->_full_cv.notify_one();
  }

  void roi_res_callback(nlohmann::json msg)
  {
    // 放入到阻塞队列中
    {
      std::unique_lock<std::mutex> lock(this->_roi_queue_mtx);
      this->_roi_queue.push(msg);
    }
    // 通知主线程取数据
    this->_roi_cv.notify_one();
  }

private:
  std::vector<std::string> _categories_filter;
  bool _keep_unlocked;
  bool _use_square_region;

  bool _locked = false;
  int _lock_thres;
  int _unlock_thres;
  int _lock_count;
  int _unlock_count;
  
  float _lock_scale_init;
  float _lock_scale;
  int _lock_scale_count;

  std::vector<std::vector<int> > _lock_rois;
  float _last_cx;
  float _last_cy;
  float _unlock_last_cx;
  float _unlock_last_cy;

  // 订阅话题
  sms::Subscriber _full_res_sub;
  sms::Subscriber _roi_res_sub;
  // 发布话题
  sms::Publisher _full_det_ctl_pub;
  sms::Publisher _roi_det_ctl_pub;
  sms::Publisher _res_pub;
  sms::Publisher _res_img_pub;

  std::mutex _full_queue_mtx;
  std::queue<nlohmann::json> _full_queue;
  std::condition_variable _full_cv;

  std::mutex _roi_queue_mtx;
  std::queue<nlohmann::json> _roi_queue;
  std::condition_variable _roi_cv;

};


}
#endif
